#include "SimEngine/GPCController.h"

#include "SimInterface/SimulatorManager.h"
#include "SimEngine/TriangleWaveform.h"
#include "SimEngine/SquareWaveform.h"
#include "SimEngine/WaveformBase.h"

#include "Utils.h"

#include <cmath>
#include <boost/smart_ptr.hpp>
#include <sstream>

using namespace RafMat;
using namespace RafMat::SimEngine;
using namespace RafMat::SimInterface;
using namespace RafMat::SimFile;
using namespace std;

class SimInput;
class Wrapper;

typedef boost::shared_ptr<GPCController> GPCControllerSPtr;
typedef boost::shared_ptr<SimInput> SimInputSPtr;

class SimInput : public WaveformBase
{
public:
	SimInput(ISimulator &isim)
		: WaveformBase(isim, "Simulink") {}
	
	void setValue(double val) { m_currentValue = val; }
	
	double getNextValue() 
	{
		return m_currentValue;
	}
	
private:
	double m_currentValue;
};

class Wrapper
{
public:
	Wrapper(int numberOfInputs)
		: m_inputsCount(numberOfInputs)
	{
		m_simulator = SimulatorManager::getSimulator();
		m_simulator->reset();

		for(int i = 0; i < m_inputsCount; ++i)
		{
			GPCControllerSPtr gpc(new GPCController(*m_simulator));
			SimInputSPtr inp(new SimInput(*m_simulator));
			m_controller.push_back(gpc);
			m_input.push_back(inp);
			gpc->getWaveformContainer()->insertWaveform(static_pointer_cast<WaveformBase>(inp));
		}
	}
	
	void setInput(int inputIndex, double val)
	{
		m_input[inputIndex]->setValue(val);
	}
	
	void setAlpha(double val)
	{
		setParameterForAll("alpha", val);
	}
	
	void setRho(double val)
	{
		setParameterForAll("rho", val);
	}
	
	void setGain(double val)
	{
		setParameterForAll("gain", val);
	}
	
	void setH(double val)
	{
		setParameterForAll("h", val);
	}
	
	void setL(double val)
	{
		setParameterForAll("l", val);
	}
	
	void setDA(double val)
	{
		setParameterForAll("da", val);
	}
	
	void setDB(double val)
	{
		setParameterForAll("db", val);
	}
	
	void setDLY(double val)
	{
		setParameterForAll("dly", val);
	}
	
	double simulate(int whichController, double y)
	{
		m_simulator->doStep();
		return m_controller[whichController]->simulateStep(y);
	}
	
	double getOutput(int whichController)
	{
		return m_controller[whichController]->currentOutput();
	}
	
	void reset(void)
	{
		m_simulator->reset();
		vector<GPCControllerSPtr>::iterator it;
		for(it = m_controller.begin(); it != m_controller.end(); ++it)
			(*it)->reset();
	}
	
private:
	void setParameterForAll(string parName, double val)
	{
		vector<GPCControllerSPtr>::iterator it;
		for(it = m_controller.begin(); it != m_controller.end(); ++it)
			(*it)->setParameter(parName, d2str(val));
	}

	string d2str(double a)
	{
		ostringstream ostr;
		ostr << a;
		return ostr.str();
	}

	unsigned m_inputsCount;

	vector<GPCControllerSPtr> m_controller;
	ISimulatorSPtr m_simulator;
	vector<SimInputSPtr> m_input;
};

#ifdef __cplusplus
extern "C" {
#endif

#define S_FUNCTION_LEVEL 2
#define S_FUNCTION_NAME  mex_gpc_controller

#include "simstruc.h"

//#define PAR(S) ssGetSFcnParam(S,0)
#define PAR(S, n) ssGetSFcnParam(S,n)

#define MDL_CHECK_PARAMETERS
#if defined(MDL_CHECK_PARAMETERS)  && defined(MATLAB_MEX_FILE)
static void mdlCheckParameters( SimStruct*S ){

	for(int i = 0; i < 8; i++)
	{
	   if( mxGetNumberOfElements(PAR(S, i)) != 1 ) {
		  ssSetErrorStatus(S,"Parametry tej S-funkcji musza byc skalarami!");
		  return;
	   }
   }
   if( mxGetPr(PAR(S, 0))[0] < 0 ){
      ssSetErrorStatus(S, "Parametr tej S-funkcji musi byc nieujemny!");
      return;
   }
}
#endif

static time_T lastTm = 0;

static void mdlInitializeSizes(SimStruct *S){

   ssSetNumSFcnParams(S, 8);  /* Number of expected parameters */
#if defined(MATLAB_MEX_FILE)
   if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
      mdlCheckParameters(S);
      if (ssGetErrorStatus(S) != NULL) {
         return;
      }
   } else {
      return; /* Parameter mismatch will be reported by Simulink */
   }
#endif
	for(int i=0; i<8; ++i)
		ssSetSFcnParamTunable(S, i, 1);
	
   if (!ssSetNumInputPorts(S, 2)) return;
   ssSetInputPortWidth(S, 0, DYNAMICALLY_SIZED);
   ssSetInputPortWidth(S, 1, DYNAMICALLY_SIZED);
   ssSetInputPortDirectFeedThrough( S, 0, 1 );
   ssSetInputPortDirectFeedThrough( S, 1, 1 );
   
   if (!ssSetNumOutputPorts(S, 1)) return;
   ssSetOutputPortWidth(S, 0, DYNAMICALLY_SIZED);

   ssSetNumSampleTimes(S, 1);
   ssSetNumPWork(S, 1); // reserve element in the pointers vector

   ssSetOptions(S, 0);
}

static void mdlInitializeSampleTimes(SimStruct *S){
   ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME);
   ssSetOffsetTime(S, 0, 0.0);
}

#define MDL_START
#if defined(MDL_START) 
static void mdlStart(SimStruct *S){

   Wrapper *w = new Wrapper(ssGetInputPortWidth(S,0));
   w->setH(*mxGetPr( PAR(S, 0) ));
   w->setL(*mxGetPr( PAR(S, 1) ));
   w->setRho(*mxGetPr( PAR(S, 2) ));
   w->setAlpha(*mxGetPr( PAR(S, 3) ));
   w->setDA(*mxGetPr( PAR(S, 4) ));
   w->setDB(*mxGetPr( PAR(S, 5) ));
   w->setDLY(*mxGetPr( PAR(S, 6) ));
   w->setGain(*mxGetPr( PAR(S, 7) ));
   w->reset();
   
   lastTm = 0;
   
   ssGetPWork(S)[0] = reinterpret_cast<void*>(w);
}
#endif

#define MDL_PROCESS_PARAMETERS
#if defined(MDL_PROCESS_PARAMETERS) && defined(MATLAB_MEX_FILE)
static void mdlProcessParameters(SimStruct *S){
   Wrapper *w = reinterpret_cast<Wrapper*>(ssGetPWork(S)[0]); 
   w->setH(*mxGetPr( PAR(S, 0) ));
   w->setL(*mxGetPr( PAR(S, 1) ));
   w->setRho(*mxGetPr( PAR(S, 2) ));
   w->setAlpha(*mxGetPr( PAR(S, 3) ));
   w->setDA(*mxGetPr( PAR(S, 4) ));
   w->setDB(*mxGetPr( PAR(S, 5) ));
   w->setDLY(*mxGetPr( PAR(S, 6) ));
   w->setGain(*mxGetPr( PAR(S, 7) ));
}
#endif

#define MDL_INITIALIZE_CONDITIONS  /* Change to #undef to remove function */
#if defined(MDL_INITIALIZE_CONDITIONS)
static void mdlInitializeConditions(SimStruct *S)
{
	Wrapper *w = reinterpret_cast<Wrapper*>(ssGetPWork(S)[0]); 
	w->reset();
}
#endif /* MDL_INITIALIZE_CONDITIONS */


static void mdlOutputs(SimStruct *S, int_T tid){
	Wrapper *w = reinterpret_cast<Wrapper*>(ssGetPWork(S)[0]); 
	
	InputRealPtrsType wskWej = ssGetInputPortRealSignalPtrs(S,0);
	InputRealPtrsType wskFdbck = ssGetInputPortRealSignalPtrs(S,1);
	real_T  *wyj = ssGetOutputPortRealSignal(S,0);
	int_T i, rozmWe = ssGetInputPortWidth(S,0), rozmFdb = ssGetInputPortWidth(S,1);

	time_T tm = ssGetT(S);

	if(rozmWe != rozmFdb)
	{
		ssSetErrorStatus(S, "Different sizes of input and feedback, c'man :(");
		return;
	}


	if(tm - lastTm >= 1.0)
	{
		for(i = 0; i < rozmWe; i++) 
		{
			w->setInput(i, *wskWej[i]);
			double res;
			res = w->simulate(i, *wskFdbck[i]);
		
			*wyj++ = Utils::limitValue(res, -1e7, 1e7);
		}
		lastTm = floor(tm);
	}
	else
	{
		for(i = 0; i < rozmWe; i++) 
			*wyj++ = w->getOutput(i);
	}
	UNUSED_ARG(tid); 
}

static void mdlTerminate(SimStruct *S){
   Wrapper *w = reinterpret_cast<Wrapper*>(ssGetPWork(S)[0]); 
   delete w;
}

#ifdef  MATLAB_MEX_FILE 
#include "simulink.c"
#else
#include "cg_sfun.h"
#endif

#ifdef __cplusplus
}
#endif